

#include <ros/ros.h>
// #include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
#include <std_msgs/Float64.h>
#include <geometry_msgs/PoseStamped.h>

double px,py,ox,oy,oz,ow;
int lastdist;
int lastdistsend;
double roadpoint[500][6];
double distancepoint;
int i=0,j=0;
const double firstdis=36;//yiquanchangdu
bool maplive=true;

void pointmeo()
{
    
    if(distancepoint<firstdis+5)
    {
        if((int)(distancepoint*100)/100!=lastdist)
        {
            roadpoint[i][0]=px;
            roadpoint[i][1]=py;
            roadpoint[i][2]=ox;
            roadpoint[i][3]=oy;
            roadpoint[i][4]=oz;
            roadpoint[i][5]=ow;
            // ROS_INFO("getpointmemory %lf, %lf",roadpoint[i][0],roadpoint[i][1]);
            i++;
            lastdist=(int)(distancepoint*100)/100;
            
        }

    }

}

void OdomCallback(const nav_msgs::Odometry& odom)
{
    px=odom.pose.pose.position.x;
    py=odom.pose.pose.position.y;
    ox=odom.pose.pose.orientation.x;
    oy=odom.pose.pose.orientation.y;
    oz=odom.pose.pose.orientation.z;
    ow=odom.pose.pose.orientation.w;
}

void DistanceCallback(const std_msgs::Float64& distance)
{
    distancepoint=distance.data;
    pointmeo();
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "pointmemory");
    ros::NodeHandle nh;
    ros::Subscriber sub_dist = nh.subscribe("/distance",1,DistanceCallback);
    ros::Subscriber sub_odom = nh.subscribe("/encoder_imu_odom",1,OdomCallback);
    ros::Publisher pub = nh.advertise<geometry_msgs::PoseStamped>( "/move_base_simple/goal", 3 );
    geometry_msgs::PoseStamped goalpoint;
    goalpoint.header.frame_id="map";
    ros::Rate r(48);
    ROS_INFO("pointmemory start successfully.");
    while(ros::ok())
    {
        ros::spinOnce();
        // if(distancepoint>firstdis-3)
        // {
        //     if(maplive)
        //     {
        //         ROS_INFO("Round 2 begins!");
        //         double time=ros::Time::now().toSec();
        //         system("rosrun map_server map_saver");
        //         maplive = false;
                
        //         system("rosnode kill map_node");
        //         popen("rosrun map_server map_server /home/racecar/map.yaml");
        //         // system("roslaunch nav_teb amcl.launch");
        //         // system("roslaunch nav_teb navigation_teb.launch");
        //         ROS_INFO("used time:%f",ros::Time::now().toSec()-time);
        //     }
        // }
        
        if(distancepoint>firstdis-0.25)
        {

            if((int)(distancepoint*100)/100!=lastdistsend && j<=i)
            {
                j++;
                goalpoint.pose.position.x=roadpoint[j][0];
                goalpoint.pose.position.y=roadpoint[j][1];
                goalpoint.pose.orientation.x=roadpoint[j][2];
                goalpoint.pose.orientation.y=roadpoint[j][3];
                goalpoint.pose.orientation.z=roadpoint[j][4];
                goalpoint.pose.orientation.w=roadpoint[j][5];
                
                lastdistsend=(int)(distancepoint*100)/100;
                pub.publish(goalpoint);
                ROS_INFO("sendpointmemory %lf, %lf ,dis: %lf",roadpoint[j][0],roadpoint[j][1],distancepoint);
            }
            
        }

        
        r.sleep();
    }
    return 0;
}